#include <stdlib.h>

#include "immorph.h"

#include <util/mem.h>
#include <util/msg.h>
//#include "../../dsp/dsp.h"

static int * __create_int_sequence(int length, int start, int increment) {
	int i, inc;
	int * shift;

	//shift = calloc(length, sizeof (int));
	shift = mem_allocn(length, int);
	for (i = 0, inc = start; i < length; i++, inc += increment) shift[i] = inc;

	return shift;
}

void imp_image_morph_dilate(ImpMatrix * Mat, ImpMatrix * SE, ImpMatrix ** Dilate) {

	real * mat = NULL;
	real * se = NULL;
	real * dilate = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;

	int d;

	real MAX;

	/* ====================================================================== */
	{
		_verify(Mat);
		_verify(SE);
		_verify(Dilate);
	}
	/* ====================================================================== */
	{
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;

	}
	/* ====================================================================== */
	{
		if (*Dilate == NULL) (*Dilate) = imp_matrix_new(rows, columns);
		dilate = (*Dilate)->data;
	}

	/* Convolve with SE ===================================================== */
	{
		int mrows = SE->rows;
		int mcolumns = SE->columns;
		int msize_minus_1 = SE->size - 1;

		int rf, cf;

		int mrc = (mrows - 1) / 2;
		int mcc = (mcolumns - 1) / 2;

		int _2columns_minus_2 = (columns << 1) - 2;
		int _2row_minus_2 = (rows << 1) - 2;

		se = SE->data;

		int * r_shift = __create_int_sequence(rows, 0, columns);

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;

				MAX = mat[i];

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					for (mc = 0; mc < mcolumns; mc++) {

						mi = _mr + mc;
						mi = msize_minus_1 - mi;

						if (!se[mi]) continue;

						// reflect x boundaries
						cf = c + (mc - mcc);
						if (cf < 0) cf = -cf;
						else if (cf >= columns) cf = _2columns_minus_2 - cf;

						d = r_shift[rf] + cf;

						if (mat[d] > MAX) MAX = mat[d];
					}
				}
				dilate[i] = MAX;
			}
		}// </editor-fold>

		free(r_shift);
	}
}

void imp_image_morph_dilate_w(ImpMatrix * Mat, int w, ImpMatrix ** Dilate) {

	ImpMatrix * SE = imp_strel_disk(w / 2.0, w);

	imp_image_morph_dilate(Mat, SE, Dilate);

	imp_object_delete(SE);
}

/*
void image_morph_dilate_zb( ImpImage * Gray, ImpMatrix * SE, ImpImage ** Dilate ) {
	
	ImpMatrix * Shift;
	real * shift;
	
	unsigned char * gray;
	unsigned char * dilate;
	real * se;
	
	int rows;
	int columns;
	long int imsize;
	
	int mrows;
	int mcolumns;
	
	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 
	int d;
	
	real MAX;
	
	gray = Gray->data;
	se = SE->data;
	
	_verify(Dilate);
	if( Dilate ) if( *Dilate == NULL) (*Dilate) = imp_image_new(Gray->width, Gray->height, IMP_IMAGE_TYPE_GRAY);
	
	dilate = (*Dilate)->data;
	
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;
	
	mrows = SE->rows;
	mcolumns = SE->columns;
	
	Shift = matrix_create_conv_index_shift(SE->rows, SE->columns, columns);
	shift = Shift->data;
	
	matrix_print(Shift);
	
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			
			MAX = gray[i];
			
			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					
					if( !se[mi] ) continue;
					
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					if( gray[d] > MAX ) MAX = gray[d];
					
				}
			}
			dilate[i] = MAX;
		}
	}
	imp_object_delete(Shift);
}
 */

